import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Point
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from math import copysign, sqrt, pow
from tf2_ros import LookupException, ConnectivityException, ExtrapolationException

class CalibrateLinear(Node):
    """校准线性移动的节点类"""
    def __init__(self, name):
        """初始化函数，设置发布器和参数"""
        super().__init__(name)
        # 创建一个发布器，用于向/cmd_vel话题发布速度命令
        self.cmd_vel = self.create_publisher(Twist, "/cmd_vel", 5)

        # 声明节点参数
        self.declare_parameter('start_test', False)  # 校准测试的起始标志
        self.declare_parameter('direction', 1.0)  # 移动方向
        self.declare_parameter('test_distance', 1.0)  # 测试距离
        self.declare_parameter('speed', 0.3)  # 速度
        self.declare_parameter('tolerance', 0.03)  # 容差
        self.declare_parameter('odom_linear_scale_correction', 1.0)  # 里程计线性比例校正因子
        self.declare_parameter('base_frame', 'base_footprint')  # 基座帧
        self.declare_parameter('odom_frame', 'odom')  # 里程计帧
        self.get_param()  # 获取参数值

        # 初始化tf监听器，用于监听坐标变换
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 初始化位置变量和起始位置
        self.position = Point()
        self.x_start = self.position.x
        self.y_start = self.position.y
        
        print("finish init work")
        # 创建定时器，周期为0.05秒
        self.timer = self.create_timer(0.05, self.on_timer)

    def get_param(self):
        """获取参数值并初始化相关变量"""
        # 获取声明的参数值
        self.start_test = self.get_parameter('start_test').get_parameter_value().bool_value
        self.direction = self.get_parameter('direction').get_parameter_value().double_value
        self.test_distance = self.get_parameter('test_distance').get_parameter_value().double_value
        self.speed = self.get_parameter('speed').get_parameter_value().double_value
        self.tolerance = self.get_parameter('tolerance').get_parameter_value().double_value
        self.odom_linear_scale_correction = self.get_parameter('odom_linear_scale_correction').get_parameter_value().double_value
        self.base_frame = self.get_parameter('base_frame').get_parameter_value().string_value
        self.odom_frame = self.get_parameter('odom_frame').get_parameter_value().string_value
        
    def on_timer(self):
        """定时器回调函数，执行移动逻辑"""
        self.get_param()
        move_cmd = Twist()
        if self.start_test:
            # 获取当前位置
            self.position.x = self.get_position().transform.translation.x
            self.position.y = self.get_position().transform.translation.y
            print("self.position.x: ", self.position.x)
            print("self.position.y: ", self.position.y)
            # 计算当前位置与起始位置的距离，并应用里程计线性比例校正因子
            distance = sqrt(pow((self.position.x - self.x_start), 2) +
                            pow((self.position.y - self.y_start), 2))
            distance *= self.odom_linear_scale_correction
            print("distance: ", distance)
            # 计算距离误差
            error = distance - self.test_distance
            print("error: ", error)

            # 如果误差在容差范围内或start_test被设置为False，则停止测试
            if not self.start_test or abs(error) < self.tolerance:
                self.start_test = rclpy.parameter.Parameter('start_test', rclpy.Parameter.Type.BOOL, False)
                all_new_parameters = [self.start_test]
                self.set_parameters(all_new_parameters)
                print("done")
            else:
                # 根据误差和方向计算线速度，并发布速度命令以调整机器人移动
                if self.direction:
                    print("x")
                    move_cmd.linear.x = copysign(self.speed, -1 * error)
                else:
                    move_cmd.linear.y = copysign(self.speed, -1 * error)
                    print("y")
            self.cmd_vel.publish(move_cmd)
        else:
            # 如果start_test为False，记录当前位置作为起始位置，并停止机器人
            self.x_start = self.get_position().transform.translation.x
            self.y_start = self.get_position().transform.translation.y
            print("self.x_start: ", self.x_start)
            print("self.y_start:", self.y_start)
            self.cmd_vel.publish(Twist())
     
    def get_position(self):
         """获取当前位置信息"""
         try:
            # 通过tf_buffer查找从里程计帧到基座帧的变换
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform(self.odom_frame, self.base_frame, now)
            return trans
         except (LookupException, ConnectivityException, ExtrapolationException):
            # 如果变换失败，记录日志并抛出异常
            self.get_logger().info('transform not ready')
            raise
            return

def main():
    """主函数，初始化节点并开始循环"""
    rclpy.init()
    class_calibratelinear = CalibrateLinear("calibrate_linear")
    rclpy.spin(class_calibratelinear)